
<!DOCTYPE html>

<html xmlns="http://www.w3.org/1999/xhtml">
  <head>
    <meta charset="utf-8" />
    <title>spatialmath.geom3d &#8212; Spatial Maths package 0.7.0
 documentation</title>
    <link rel="stylesheet" href="../../_static/alabaster.css" type="text/css" />
    <link rel="stylesheet" href="../../_static/pygments.css" type="text/css" />
    <link rel="stylesheet" type="text/css" href="../../_static/graphviz.css" />
    <script id="documentation_options" data-url_root="../../" src="../../_static/documentation_options.js"></script>
    <script src="../../_static/jquery.js"></script>
    <script src="../../_static/underscore.js"></script>
    <script src="../../_static/doctools.js"></script>
    <script src="../../_static/language_data.js"></script>
    <script async="async" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/latest.js?config=TeX-AMS-MML_HTMLorMML"></script>
    <link rel="index" title="Index" href="../../genindex.html" />
    <link rel="search" title="Search" href="../../search.html" />
   
  <link rel="stylesheet" href="../../_static/custom.css" type="text/css" />
  
  
  <meta name="viewport" content="width=device-width, initial-scale=0.9, maximum-scale=0.9" />

  </head><body>
  

    <div class="document">
      <div class="documentwrapper">
        <div class="bodywrapper">
          

          <div class="body" role="main">
            
  <h1>Source code for spatialmath.geom3d</h1><div class="highlight"><pre>
<span></span><span class="ch">#!/usr/bin/env python3</span>

<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="kn">import</span> <span class="nn">math</span>
<span class="kn">from</span> <span class="nn">collections</span> <span class="kn">import</span> <span class="n">namedtuple</span>
<span class="kn">from</span> <span class="nn">collections</span> <span class="kn">import</span> <span class="n">UserList</span>

<span class="kn">import</span> <span class="nn">spatialmath.base.argcheck</span> <span class="k">as</span> <span class="nn">arg</span>
<span class="kn">import</span> <span class="nn">spatialmath.base</span> <span class="k">as</span> <span class="nn">sm</span>
<span class="kn">import</span> <span class="nn">matplotlib.pyplot</span> <span class="k">as</span> <span class="nn">plt</span>
<span class="kn">from</span> <span class="nn">mpl_toolkits.mplot3d</span> <span class="kn">import</span> <span class="n">Axes3D</span>
<span class="kn">from</span> <span class="nn">spatialmath</span> <span class="kn">import</span> <span class="n">SE3</span>

<span class="n">_eps</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">finfo</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">float64</span><span class="p">)</span><span class="o">.</span><span class="n">eps</span>

   
<div class="viewcode-block" id="Plane"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plane">[docs]</a><span class="k">class</span> <span class="nc">Plane</span><span class="p">:</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create a plane object from linear coefficients</span>
<span class="sd">    </span>
<span class="sd">    :param c: Plane coefficients</span>
<span class="sd">    :type c: 4-element array_like</span>
<span class="sd">    :return: a Plane object</span>
<span class="sd">    :rtype: Plane</span>

<span class="sd">    Planes are represented by the 4-vector :math:`[a, b, c, d]` which describes</span>
<span class="sd">    the plane :math:`\pi: ax + by + cz + d=0`.</span>
<span class="sd">    &quot;&quot;&quot;</span>
<div class="viewcode-block" id="Plane.__init__"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plane.__init__">[docs]</a>    <span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">c</span><span class="p">):</span>

        <span class="bp">self</span><span class="o">.</span><span class="n">plane</span> <span class="o">=</span> <span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">c</span><span class="p">,</span> <span class="mi">4</span><span class="p">)</span></div>
    
    <span class="c1"># point and normal</span>
<div class="viewcode-block" id="Plane.PN"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plane.PN">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">PN</span><span class="p">(</span><span class="n">p</span><span class="p">,</span> <span class="n">n</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Create a plane object from point and normal</span>
<span class="sd">        </span>
<span class="sd">        :param p: Point in the plane</span>
<span class="sd">        :type p: 3-element array_like</span>
<span class="sd">        :param n: Normal to the plane</span>
<span class="sd">        :type n: 3-element array_like</span>
<span class="sd">        :return: a Plane object</span>
<span class="sd">        :rtype: Plane</span>

<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">n</span> <span class="o">=</span> <span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">n</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span>  <span class="c1"># normal to the plane</span>
        <span class="n">p</span> <span class="o">=</span> <span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">p</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span>  <span class="c1"># point on the plane</span>
        <span class="k">return</span> <span class="n">Plane</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">r_</span><span class="p">[</span><span class="n">n</span><span class="p">,</span> <span class="o">-</span><span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">n</span><span class="p">,</span> <span class="n">p</span><span class="p">)])</span></div>
    
    <span class="c1"># point and normal</span>
<div class="viewcode-block" id="Plane.P3"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plane.P3">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">P3</span><span class="p">(</span><span class="n">p</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Create a plane object from three points</span>
<span class="sd">        </span>
<span class="sd">        :param p: Three points in the plane</span>
<span class="sd">        :type p: numpy.ndarray, shape=(3,3)</span>
<span class="sd">        :return: a Plane object</span>
<span class="sd">        :rtype: Plane</span>
<span class="sd">        &quot;&quot;&quot;</span>
        
        <span class="n">p</span> <span class="o">=</span> <span class="n">arg</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span><span class="mi">3</span><span class="p">))</span>
        <span class="n">v1</span> <span class="o">=</span> <span class="n">p</span><span class="p">[:,</span><span class="mi">0</span><span class="p">]</span>
        <span class="n">v2</span> <span class="o">=</span> <span class="n">p</span><span class="p">[:,</span><span class="mi">1</span><span class="p">]</span>
        <span class="n">v3</span> <span class="o">=</span> <span class="n">p</span><span class="p">[:,</span><span class="mi">2</span><span class="p">]</span>
        
        <span class="c1"># compute a normal</span>
        <span class="n">n</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">v2</span><span class="o">-</span><span class="n">v1</span><span class="p">,</span> <span class="n">v3</span><span class="o">-</span><span class="n">v1</span><span class="p">)</span>
        
        <span class="k">return</span> <span class="n">Plane</span><span class="p">(</span><span class="n">n</span><span class="p">,</span> <span class="n">v1</span><span class="p">)</span></div>
        
    <span class="c1"># line and point</span>
    <span class="c1"># 3 points</span>
        
    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">n</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Normal to the plane</span>
<span class="sd">        </span>
<span class="sd">        :return: Normal to the plane</span>
<span class="sd">        :rtype: 3-element array_like</span>
<span class="sd">        </span>
<span class="sd">        For a plane :math:`\pi: ax + by + cz + d=0` this is the vector</span>
<span class="sd">        :math:`[a,b,c]`.</span>

<span class="sd">        &quot;&quot;&quot;</span>
        <span class="c1"># normal</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">plane</span><span class="p">[:</span><span class="mi">3</span><span class="p">]</span>
    
    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">d</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Plane offset</span>
<span class="sd">        </span>
<span class="sd">        :return: Offset of the plane</span>
<span class="sd">        :rtype: float</span>
<span class="sd">        </span>
<span class="sd">        For a plane :math:`\pi: ax + by + cz + d=0` this is the scalar</span>
<span class="sd">        :math:`d`.</span>

<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">plane</span><span class="p">[</span><span class="mi">3</span><span class="p">]</span>
    
<div class="viewcode-block" id="Plane.contains"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plane.contains">[docs]</a>    <span class="k">def</span> <span class="nf">contains</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">p</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="mi">10</span><span class="o">*</span><span class="n">_eps</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        </span>
<span class="sd">        :param p: A 3D point</span>
<span class="sd">        :type p: 3-element array_like</span>
<span class="sd">        :param tol: Tolerance, defaults to 10*_eps</span>
<span class="sd">        :type tol: float, optional</span>
<span class="sd">        :return: if the point is in the plane</span>
<span class="sd">        :rtype: bool</span>

<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="nb">abs</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">n</span><span class="p">,</span> <span class="n">p</span><span class="p">)</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">d</span><span class="p">)</span> <span class="o">&lt;</span> <span class="n">tol</span></div>
    
    <span class="k">def</span> <span class="fm">__str__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        </span>
<span class="sd">        :return: String representation of plane</span>
<span class="sd">        :rtype: str</span>

<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="nb">str</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">plane</span><span class="p">)</span></div>

<div class="viewcode-block" id="Plucker"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker">[docs]</a><span class="k">class</span> <span class="nc">Plucker</span><span class="p">(</span><span class="n">UserList</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Plucker coordinate class</span>
<span class="sd">    </span>
<span class="sd">    Concrete class to represent a 3D line using Plucker coordinates.</span>
<span class="sd">    </span>
<span class="sd">    Methods:</span>
<span class="sd">        </span>
<span class="sd">    Plucker            Contructor from points</span>
<span class="sd">    Plucker.planes     Constructor from planes</span>
<span class="sd">    Plucker.pointdir   Constructor from point and direction</span>
<span class="sd">    </span>
<span class="sd">    Information and test methods::</span>
<span class="sd">    closest            closest point on line</span>
<span class="sd">    commonperp         common perpendicular for two lines</span>
<span class="sd">    contains           test if point is on line</span>
<span class="sd">    distance           minimum distance between two lines</span>
<span class="sd">    intersects         intersection point for two lines</span>
<span class="sd">    intersect_plane    intersection points with a plane</span>
<span class="sd">    intersect_volume   intersection points with a volume</span>
<span class="sd">    pp                 principal point</span>
<span class="sd">    ppd                principal point distance from origin</span>
<span class="sd">    point              generate point on line</span>
<span class="sd">    </span>
<span class="sd">    Conversion methods::</span>
<span class="sd">    char               convert to human readable string</span>
<span class="sd">    double             convert to 6-vector</span>
<span class="sd">    skew               convert to 4x4 skew symmetric matrix</span>
<span class="sd">    </span>
<span class="sd">    Display and print methods::</span>
<span class="sd">    display            display in human readable form</span>
<span class="sd">    plot               plot line</span>
<span class="sd">    </span>
<span class="sd">    Operators:</span>
<span class="sd">    *                  multiply Plucker matrix by a general matrix</span>
<span class="sd">    |                  test if lines are parallel</span>
<span class="sd">    ^                  test if lines intersect</span>
<span class="sd">    ==                 test if two lines are equivalent</span>
<span class="sd">    ~=                 test if lines are not equivalent</span>

<span class="sd">    Notes:</span>
<span class="sd">        </span>
<span class="sd">     - This is reference (handle) class object</span>
<span class="sd">     - Plucker objects can be used in vectors and arrays</span>
<span class="sd">    </span>
<span class="sd">    References:</span>
<span class="sd">        </span>
<span class="sd">     - Ken Shoemake, &quot;Ray Tracing News&quot;, Volume 11, Number 1</span>
<span class="sd">       http://www.realtimerendering.com/resources/RTNews/html/rtnv11n1.html#art3</span>
<span class="sd">     - Matt Mason lecture notes http://www.cs.cmu.edu/afs/cs/academic/class/16741-s07/www/lectures/lecture9.pdf</span>
<span class="sd">     - Robotics, Vision &amp; Control: Second Edition, P. Corke, Springer 2016; p596-7.</span>
<span class="sd">    </span>
<span class="sd">    Implementation notes:</span>
<span class="sd">        </span>
<span class="sd">     - The internal representation is a 6-vector [v, w] where v (moment), w (direction).</span>
<span class="sd">     - There is a huge variety of notation used across the literature, as well as the ordering</span>
<span class="sd">       of the direction and moment components in the 6-vector.</span>
<span class="sd">    </span>
<span class="sd">    Copyright (C) 1993-2019 Peter I. Corke</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="c1"># w  # direction vector</span>
    <span class="c1"># v  # moment vector (normal of plane containing line and origin)</span>
    
<div class="viewcode-block" id="Plucker.__init__"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.__init__">[docs]</a>    <span class="k">def</span> <span class="fm">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">v</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">w</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Create a Plucker 3D line object</span>
<span class="sd">        </span>
<span class="sd">        :param v: Plucker vector, Plucker object, Plucker moment</span>
<span class="sd">        :type v: 6-element array_like, Plucker instance, 3-element array_like</span>
<span class="sd">        :param w: Plucker direction, optional</span>
<span class="sd">        :type w: 3-element array_like, optional</span>
<span class="sd">        :raises ValueError: bad arguments</span>
<span class="sd">        :return: Plucker line</span>
<span class="sd">        :rtype: Plucker</span>

<span class="sd">        - ``L = Plucker(X)`` creates a Plucker object from the Plucker coordinate vector</span>
<span class="sd">          ``X`` = [V,W] where V (3-vector) is the moment and W (3-vector) is the line direction.</span>

<span class="sd">        - ``L = Plucker(L)`` creates a copy of the Plucker object ``L``.</span>
<span class="sd">        </span>
<span class="sd">        - ``L = Plucker(V, W)`` creates a Plucker object from moment ``V`` (3-vector) and</span>
<span class="sd">          line direction ``W`` (3-vector).</span>
<span class="sd">          </span>
<span class="sd">        Notes:</span>
<span class="sd">            </span>
<span class="sd">        - The Plucker object inherits from ``collections.UserList`` and has list-like</span>
<span class="sd">          behaviours.</span>
<span class="sd">        - A single Plucker object contains a 1D array of Plucker coordinates.</span>
<span class="sd">        - The elements of the array are guaranteed to be Plucker coordinates.</span>
<span class="sd">        - The number of elements is given by ``len(L)``</span>
<span class="sd">        - The elements can be accessed using index and slice notation, eg. ``L[1]`` or</span>
<span class="sd">          ``L[2:3]``</span>
<span class="sd">        - The Plucker instance can be used as an iterator in a for loop or list comprehension.</span>
<span class="sd">        - Some methods support operations on the internal list.</span>
<span class="sd">          </span>
<span class="sd">        :seealso: Plucker.PQ, Plucker.Planes, Plucker.PointDir</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="nb">super</span><span class="p">()</span><span class="o">.</span><span class="fm">__init__</span><span class="p">()</span>  <span class="c1"># enable list powers</span>
        <span class="k">if</span> <span class="n">w</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
            <span class="c1"># single parameter</span>
            <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">v</span><span class="p">,</span> <span class="n">Plucker</span><span class="p">):</span>
                <span class="bp">self</span><span class="o">.</span><span class="n">data</span> <span class="o">=</span> <span class="p">[</span><span class="n">v</span><span class="o">.</span><span class="n">A</span><span class="p">]</span>
            <span class="k">elif</span> <span class="n">arg</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">v</span><span class="p">,</span> <span class="mi">6</span><span class="p">):</span>
                <span class="n">pl</span> <span class="o">=</span> <span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">v</span><span class="p">)</span>
                <span class="bp">self</span><span class="o">.</span><span class="n">data</span> <span class="o">=</span> <span class="p">[</span><span class="n">pl</span><span class="p">]</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;bad argument&#39;</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">assert</span> <span class="n">arg</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">v</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span> <span class="ow">and</span> <span class="n">arg</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">w</span><span class="p">,</span> <span class="mi">3</span><span class="p">),</span> <span class="s1">&#39;expecting two 3-vectors&#39;</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">data</span> <span class="o">=</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">r_</span><span class="p">[</span><span class="n">v</span><span class="p">,</span> <span class="n">w</span><span class="p">]]</span></div>
            
        <span class="c1"># needed to allow __rmul__ to work if left multiplied by ndarray</span>
        <span class="c1">#self.__array_priority__ = 100  </span>


<div class="viewcode-block" id="Plucker.PQ"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.PQ">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">PQ</span><span class="p">(</span><span class="n">P</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">Q</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Create Plucker line object from two 3D points</span>
<span class="sd">        </span>
<span class="sd">        :param P: First 3D point</span>
<span class="sd">        :type P: 3-element array_like</span>
<span class="sd">        :param Q: Second 3D point</span>
<span class="sd">        :type Q: 3-element array_like</span>
<span class="sd">        :return: Plucker line</span>
<span class="sd">        :rtype: Plucker</span>

<span class="sd">        ``L = Plucker(P, Q)`` create a Plucker object that represents</span>
<span class="sd">        the line joining the 3D points ``P`` (3-vector) and ``Q`` (3-vector). The direction</span>
<span class="sd">        is from ``Q`` to ``P``.</span>

<span class="sd">        :seealso: Plucker, Plucker.Planes, Plucker.PointDir</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">P</span> <span class="o">=</span> <span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">P</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span>
        <span class="n">Q</span> <span class="o">=</span> <span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">Q</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span>
        <span class="c1"># compute direction and moment</span>
        <span class="n">w</span> <span class="o">=</span> <span class="n">P</span> <span class="o">-</span> <span class="n">Q</span>
        <span class="n">v</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">P</span> <span class="o">-</span> <span class="n">Q</span><span class="p">,</span> <span class="n">P</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">Plucker</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">r_</span><span class="p">[</span><span class="n">v</span><span class="p">,</span> <span class="n">w</span><span class="p">])</span></div>
    
<div class="viewcode-block" id="Plucker.Planes"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.Planes">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">Planes</span><span class="p">(</span><span class="n">pi1</span><span class="p">,</span> <span class="n">pi2</span><span class="p">):</span>
        <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Create Plucker line from two planes</span>
<span class="sd">                </span>
<span class="sd">        :param pi1: First plane</span>
<span class="sd">        :type pi1: 4-element array_like, or Plane</span>
<span class="sd">        :param pi2: Second plane</span>
<span class="sd">        :type pi2: 4-element array_like, or Plane</span>
<span class="sd">        :return: Plucker line</span>
<span class="sd">        :rtype: Plucker</span>

<span class="sd">        ``L = Plucker.planes(PI1, PI2)`` is a Plucker object that represents</span>
<span class="sd">        the line formed by the intersection of two planes ``PI1`` and ``PI2``.</span>

<span class="sd">        Planes are represented by the 4-vector :math:`[a, b, c, d]` which describes</span>
<span class="sd">        the plane :math:`\pi: ax + by + cz + d=0`.</span>
<span class="sd">           </span>
<span class="sd">        :seealso: Plucker, Plucker.PQ, Plucker.PointDir</span>
<span class="sd">        &quot;&quot;&quot;</span>

        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">pi1</span><span class="p">,</span> <span class="n">Plane</span><span class="p">):</span>
            <span class="n">pi1</span> <span class="o">=</span> <span class="n">Plane</span><span class="p">(</span><span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">pi1</span><span class="p">,</span> <span class="mi">4</span><span class="p">))</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">pi2</span><span class="p">,</span> <span class="n">Plane</span><span class="p">):</span>
            <span class="n">pi2</span> <span class="o">=</span> <span class="n">Plane</span><span class="p">(</span><span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">pi2</span><span class="p">,</span> <span class="mi">4</span><span class="p">))</span>
        
        <span class="n">w</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">pi1</span><span class="o">.</span><span class="n">n</span><span class="p">,</span> <span class="n">pi2</span><span class="o">.</span><span class="n">n</span><span class="p">)</span>
        <span class="n">v</span> <span class="o">=</span> <span class="n">pi2</span><span class="o">.</span><span class="n">d</span> <span class="o">*</span> <span class="n">pi1</span><span class="o">.</span><span class="n">n</span> <span class="o">-</span> <span class="n">pi1</span><span class="o">.</span><span class="n">d</span> <span class="o">*</span> <span class="n">pi2</span><span class="o">.</span><span class="n">n</span>
        <span class="k">return</span> <span class="n">Plucker</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">r_</span><span class="p">[</span><span class="n">v</span><span class="p">,</span> <span class="n">w</span><span class="p">])</span></div>

<div class="viewcode-block" id="Plucker.PointDir"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.PointDir">[docs]</a>    <span class="nd">@staticmethod</span>
    <span class="k">def</span> <span class="nf">PointDir</span><span class="p">(</span><span class="n">point</span><span class="p">,</span> <span class="nb">dir</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Create Plucker line from point and direction</span>
<span class="sd">        </span>
<span class="sd">        :param point: A 3D point</span>
<span class="sd">        :type point: 3-element array_like</span>
<span class="sd">        :param dir: Direction vector</span>
<span class="sd">        :type dir: 3-element array_like</span>
<span class="sd">        :return: Plucker line</span>
<span class="sd">        :rtype: Plucker</span>
<span class="sd">        </span>
<span class="sd">        ``L = Plucker.pointdir(P, W)`` is a Plucker object that represents the</span>
<span class="sd">        line containing the point ``P`` and parallel to the direction vector ``W``.</span>

<span class="sd">        :seealso: Plucker, Plucker.Planes, Plucker.PQ</span>
<span class="sd">        &quot;&quot;&quot;</span>

        <span class="n">point</span> <span class="o">=</span> <span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">point</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span>
        <span class="nb">dir</span> <span class="o">=</span> <span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="nb">dir</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span>
        
        <span class="k">return</span> <span class="n">Plucker</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">r_</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="nb">dir</span><span class="p">,</span> <span class="n">point</span><span class="p">),</span> <span class="nb">dir</span><span class="p">])</span></div>
    
<div class="viewcode-block" id="Plucker.append"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.append">[docs]</a>    <span class="k">def</span> <span class="nf">append</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        </span>
<span class="sd">        :param x: Plucker object</span>
<span class="sd">        :type x: Plucker</span>
<span class="sd">        :raises ValueError: Attempt to append a non Plucker object</span>
<span class="sd">        :return: Plucker object with new Plucker line appended</span>
<span class="sd">        :rtype: Plucker</span>

<span class="sd">        &quot;&quot;&quot;</span>
        <span class="c1">#print(&#39;in append method&#39;)</span>
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">type</span><span class="p">(</span><span class="bp">self</span><span class="p">)</span> <span class="o">==</span> <span class="nb">type</span><span class="p">(</span><span class="n">x</span><span class="p">):</span>
            <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s2">&quot;can pnly append Plucker object&quot;</span><span class="p">)</span>
        <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="n">x</span><span class="p">)</span> <span class="o">&gt;</span> <span class="mi">1</span><span class="p">:</span>
            <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s2">&quot;cant append a Plucker sequence - use extend&quot;</span><span class="p">)</span>
        <span class="nb">super</span><span class="p">()</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">x</span><span class="o">.</span><span class="n">A</span><span class="p">)</span></div>

    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">A</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="c1"># get the underlying numpy array</span>
        <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">data</span><span class="p">)</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">data</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">data</span>

    <span class="k">def</span> <span class="fm">__getitem__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">i</span><span class="p">):</span>
        <span class="c1"># print(&#39;getitem&#39;, i, &#39;class&#39;, self.__class__)</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="vm">__class__</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">data</span><span class="p">[</span><span class="n">i</span><span class="p">])</span>
    
    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">v</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Moment vector</span>
<span class="sd">        </span>
<span class="sd">        :return: the moment vector</span>
<span class="sd">        :rtype: numpy.ndarray, shape=(3,)</span>

<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">data</span><span class="p">[</span><span class="mi">0</span><span class="p">][</span><span class="mi">0</span><span class="p">:</span><span class="mi">3</span><span class="p">]</span>
    
    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">w</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Direction vector</span>
<span class="sd">        </span>
<span class="sd">        :return: the direction vector</span>
<span class="sd">        :rtype: numpy.ndarray, shape=(3,)</span>
<span class="sd">        </span>
<span class="sd">        :seealso: Plucker.uw</span>

<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="bp">self</span><span class="o">.</span><span class="n">data</span><span class="p">[</span><span class="mi">0</span><span class="p">][</span><span class="mi">3</span><span class="p">:</span><span class="mi">6</span><span class="p">]</span>
    
    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">uw</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Line direction as a unit vector</span>
<span class="sd">        </span>
<span class="sd">        :return: Line direction</span>
<span class="sd">        :rtype: numpy.ndarray, shape=(3,)</span>

<span class="sd">        ``line.uw`` is a unit-vector parallel to the line.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">sm</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">)</span>
    
    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">vec</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Line as a Plucker coordinate vector</span>
<span class="sd">        </span>
<span class="sd">        :return: Coordinate vector</span>
<span class="sd">        :rtype: numpy.ndarray, shape=(6,)</span>
<span class="sd">        </span>
<span class="sd">        ``line.vec`` is the  Plucker coordinate vector ``X`` = [V,W] where V (3-vector)</span>
<span class="sd">        is the moment and W (3-vector) is the line direction.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">r_</span><span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">v</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">]</span>
    
    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">skew</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Line as a Plucker skew-matrix</span>
<span class="sd">        </span>
<span class="sd">        :return: Skew-symmetric matrix form of Plucker coordinates</span>
<span class="sd">        :rtype: numpy.ndarray, shape=(4,4)</span>

<span class="sd">        ``M = line.skew()`` is the Plucker matrix, a 4x4 skew-symmetric matrix</span>
<span class="sd">        representation of the line.</span>

<span class="sd">        Notes:</span>
<span class="sd">            </span>
<span class="sd">         - For two homogeneous points P and Q on the line, :math:`PQ^T-QP^T` is also skew</span>
<span class="sd">           symmetric.</span>
<span class="sd">         - The projection of Plucker line by a perspective camera is a homogeneous line (3x1)</span>
<span class="sd">           given by :math:`\vee C M C^T` where :math:`C \in \mathbf{R}^{3 \times 4}` is the camera matrix.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        
        <span class="n">v</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">v</span><span class="p">;</span> <span class="n">w</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">;</span>
        
        <span class="c1"># the following matrix is at odds with H&amp;Z pg. 72</span>
        <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span>
                <span class="p">[</span> <span class="mi">0</span><span class="p">,</span>     <span class="n">v</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="o">-</span><span class="n">v</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">w</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span>
                <span class="p">[</span><span class="o">-</span><span class="n">v</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span>  <span class="mi">0</span> <span class="p">,</span>    <span class="n">v</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">w</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span>
                <span class="p">[</span> <span class="n">v</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="o">-</span><span class="n">v</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span>  <span class="mi">0</span><span class="p">,</span>    <span class="n">w</span><span class="p">[</span><span class="mi">2</span><span class="p">]],</span>
                <span class="p">[</span><span class="o">-</span><span class="n">w</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="o">-</span><span class="n">w</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="o">-</span><span class="n">w</span><span class="p">[</span><span class="mi">2</span><span class="p">],</span> <span class="mi">0</span>   <span class="p">]</span>
            <span class="p">])</span>
    
    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">pp</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Principal point of the line</span>

<span class="sd">        ``line.pp`` is the point on the line that is closest to the origin.</span>

<span class="sd">        Notes:</span>
<span class="sd">            </span>
<span class="sd">         - Same as Plucker.point(0)</span>

<span class="sd">        :seealso: Plucker.ppd, Plucker.point</span>
<span class="sd">        &quot;&quot;&quot;</span>
        
        <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">v</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">)</span> <span class="o">/</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">)</span>    
    <span class="nd">@property</span>
    <span class="k">def</span> <span class="nf">ppd</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Distance from principal point to the origin</span>

<span class="sd">        :return: Distance from principal point to the origin</span>
<span class="sd">        :rtype: float</span>
<span class="sd">        </span>
<span class="sd">        ``line.ppd`` is the distance from the principal point to the origin.</span>
<span class="sd">        This is the smallest distance of any point on the line</span>
<span class="sd">        to the origin.</span>

<span class="sd">        :seealso: Plucker.pp</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">v</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">v</span><span class="p">)</span> <span class="o">/</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">)</span> <span class="p">)</span>

<div class="viewcode-block" id="Plucker.point"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.point">[docs]</a>    <span class="k">def</span> <span class="nf">point</span><span class="p">(</span><span class="n">L</span><span class="p">,</span> <span class="n">lam</span><span class="p">):</span>
        <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Generate point on line</span>
<span class="sd">       </span>
<span class="sd">        :param lam: Scalar distance from principal point</span>
<span class="sd">        :type lam: float</span>
<span class="sd">        :return: Distance from principal point to the origin</span>
<span class="sd">        :rtype: float</span>

<span class="sd">        ``line.point(LAMBDA)`` is a point on the line, where ``LAMBDA`` is the parametric</span>
<span class="sd">        distance along the line from the principal point of the line such</span>
<span class="sd">        that :math:`P = P_p + \lambda \hat{d}` and :math:`\hat{d}` is the line</span>
<span class="sd">        direction given by ``line.uw``.</span>

<span class="sd">        :seealso: Plucker.pp, Plucker.closest, Plucker.uw</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">lam</span> <span class="o">=</span> <span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">lam</span><span class="p">,</span> <span class="n">out</span><span class="o">=</span><span class="s1">&#39;row&#39;</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">L</span><span class="o">.</span><span class="n">pp</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span><span class="mi">1</span><span class="p">))</span> <span class="o">+</span> <span class="n">L</span><span class="o">.</span><span class="n">uw</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span><span class="mi">1</span><span class="p">))</span> <span class="o">*</span> <span class="n">lam</span></div>

    <span class="c1"># ------------------------------------------------------------------------- #</span>
    <span class="c1">#  TESTS ON PLUCKER OBJECTS</span>
    <span class="c1"># ------------------------------------------------------------------------- #</span>

<div class="viewcode-block" id="Plucker.contains"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.contains">[docs]</a>    <span class="k">def</span> <span class="nf">contains</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">x</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="mi">50</span><span class="o">*</span><span class="n">_eps</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Test if points are on the line</span>
<span class="sd">        </span>
<span class="sd">        :param x: 3D point</span>
<span class="sd">        :type x: 3-element array_like, or numpy.ndarray, shape=(3,N)</span>
<span class="sd">        :param tol: Tolerance, defaults to 50*_eps</span>
<span class="sd">        :type tol: float, optional</span>
<span class="sd">        :raises ValueError: Bad argument</span>
<span class="sd">        :return: Whether point is on the line</span>
<span class="sd">        :rtype: bool or numpy.ndarray(N) of bool</span>

<span class="sd">        ``line.contains(X)`` is true if the point ``X`` lies on the line defined by</span>
<span class="sd">        the Plucker object self.</span>
<span class="sd">        </span>
<span class="sd">        If ``X`` is an array with 3 rows, the test is performed on every column and</span>
<span class="sd">        an array of booleans is returned.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="n">arg</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="mi">3</span><span class="p">):</span>
            <span class="n">x</span> <span class="o">=</span> <span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">x</span><span class="p">)</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">x</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">pp</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">)</span> <span class="p">)</span> <span class="o">&lt;</span> <span class="n">tol</span>
        <span class="k">elif</span> <span class="n">arg</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="p">(</span><span class="mi">3</span><span class="p">,</span><span class="kc">None</span><span class="p">)):</span>
            <span class="k">return</span> <span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">_</span> <span class="o">-</span> <span class="bp">self</span><span class="o">.</span><span class="n">pp</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">w</span><span class="p">))</span> <span class="o">&lt;</span> <span class="n">tol</span> <span class="k">for</span> <span class="n">_</span> <span class="ow">in</span> <span class="n">x</span><span class="o">.</span><span class="n">T</span><span class="p">]</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;bad argument&#39;</span><span class="p">)</span></div>

<div class="viewcode-block" id="Plucker.__eq__"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.__eq__">[docs]</a>    <span class="k">def</span> <span class="fm">__eq__</span><span class="p">(</span><span class="n">l1</span><span class="p">,</span> <span class="n">l2</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Test if two lines are equivalent</span>
<span class="sd">        </span>
<span class="sd">        :param l1: First line</span>
<span class="sd">        :type l1: Plucker</span>
<span class="sd">        :param l2: Second line</span>
<span class="sd">        :type l2: Plucker</span>
<span class="sd">        :return: Plucker</span>
<span class="sd">        :return: line equivalence</span>
<span class="sd">        :rtype: bool</span>

<span class="sd">        ``L1 == L2`` is true if the Plucker objects describe the same line in</span>
<span class="sd">        space.  Note that because of the over parameterization, lines can be</span>
<span class="sd">        equivalent even if their coordinate vectors are different.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="nb">abs</span><span class="p">(</span> <span class="mi">1</span> <span class="o">-</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">sm</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="n">l1</span><span class="o">.</span><span class="n">vec</span><span class="p">),</span> <span class="n">sm</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="n">l2</span><span class="o">.</span><span class="n">vec</span><span class="p">)))</span> <span class="o">&lt;</span> <span class="mi">10</span><span class="o">*</span><span class="n">_eps</span></div>
    
<div class="viewcode-block" id="Plucker.__ne__"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.__ne__">[docs]</a>    <span class="k">def</span> <span class="fm">__ne__</span><span class="p">(</span><span class="n">l1</span><span class="p">,</span> <span class="n">l2</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Test if two lines are not equivalent</span>
<span class="sd">        </span>
<span class="sd">        :param l1: First line</span>
<span class="sd">        :type l1: Plucker</span>
<span class="sd">        :param l2: Second line</span>
<span class="sd">        :type l2: Plucker</span>
<span class="sd">        :return: line inequivalence</span>
<span class="sd">        :rtype: bool</span>

<span class="sd">        ``L1 != L2`` is true if the Plucker objects describe different lines in</span>
<span class="sd">        space.  Note that because of the over parameterization, lines can be</span>
<span class="sd">        equivalent even if their coordinate vectors are different.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        
        <span class="k">return</span> <span class="ow">not</span> <span class="n">l1</span><span class="o">.</span><span class="fm">__eq__</span><span class="p">(</span><span class="n">l2</span><span class="p">)</span></div>
    
<div class="viewcode-block" id="Plucker.isparallel"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.isparallel">[docs]</a>    <span class="k">def</span> <span class="nf">isparallel</span><span class="p">(</span><span class="n">l1</span><span class="p">,</span> <span class="n">l2</span><span class="p">,</span> <span class="n">tol</span><span class="o">=</span><span class="mi">10</span><span class="o">*</span><span class="n">_eps</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Test if lines are parallel</span>
<span class="sd">        </span>
<span class="sd">        :param l1: First line</span>
<span class="sd">        :type l1: Plucker</span>
<span class="sd">        :param l2: Second line</span>
<span class="sd">        :type l2: Plucker</span>
<span class="sd">        :return: lines are parallel</span>
<span class="sd">        :rtype: bool</span>

<span class="sd">        ``l1.isparallel(l2)`` is true if the two lines are parallel.</span>
<span class="sd">        </span>
<span class="sd">        ``l1 | l2`` as above but in binary operator form</span>

<span class="sd">        :seealso: Plucker.or, Plucker.intersects</span>
<span class="sd">        &quot;&quot;&quot;</span>
        
        <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">l1</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">l2</span><span class="o">.</span><span class="n">w</span><span class="p">)</span> <span class="p">)</span> <span class="o">&lt;</span> <span class="n">tol</span></div>

    
<div class="viewcode-block" id="Plucker.__or__"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.__or__">[docs]</a>    <span class="k">def</span> <span class="fm">__or__</span><span class="p">(</span><span class="n">l1</span><span class="p">,</span> <span class="n">l2</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Test if lines are parallel as a binary operator</span>
<span class="sd">        </span>
<span class="sd">        :param l1: First line</span>
<span class="sd">        :type l1: Plucker</span>
<span class="sd">        :param l2: Second line</span>
<span class="sd">        :type l2: Plucker</span>
<span class="sd">        :return: lines are parallel</span>
<span class="sd">        :rtype: bool</span>

<span class="sd">        ``l1 | l2`` is an operator which is true if the two lines are parallel.</span>

<span class="sd">        :seealso: Plucker.isparallel, Plucker.__xor__</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="n">l1</span><span class="o">.</span><span class="n">isparallel</span><span class="p">(</span><span class="n">l2</span><span class="p">)</span></div>

    
<div class="viewcode-block" id="Plucker.__xor__"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.__xor__">[docs]</a>    <span class="k">def</span> <span class="fm">__xor__</span><span class="p">(</span><span class="n">l1</span><span class="p">,</span> <span class="n">l2</span><span class="p">):</span>
        
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Test if lines intersect as a binary operator</span>
<span class="sd">        </span>
<span class="sd">        :param l1: First line</span>
<span class="sd">        :type l1: Plucker</span>
<span class="sd">        :param l2: Second line</span>
<span class="sd">        :type l2: Plucker</span>
<span class="sd">        :return: lines intersect</span>
<span class="sd">        :rtype: bool</span>

<span class="sd">        ``l1 ^ l2`` is an operator which is true if the two lines intersect at a point.</span>

<span class="sd">        Notes:</span>
<span class="sd">            </span>
<span class="sd">         - Is false if the lines are equivalent since they would intersect at</span>
<span class="sd">           an infinite number of points.</span>

<span class="sd">        :seealso: Plucker.intersects, Plucker.parallel</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">return</span> <span class="ow">not</span> <span class="n">l1</span><span class="o">.</span><span class="n">isparallel</span><span class="p">(</span><span class="n">l2</span><span class="p">)</span> <span class="ow">and</span> <span class="p">(</span><span class="nb">abs</span><span class="p">(</span><span class="n">l1</span> <span class="o">*</span> <span class="n">l2</span><span class="p">)</span> <span class="o">&lt;</span> <span class="mi">10</span><span class="o">*</span><span class="n">_eps</span> <span class="p">)</span></div>
    
    <span class="c1"># ------------------------------------------------------------------------- #</span>
    <span class="c1">#  PLUCKER LINE DISTANCE AND INTERSECTION</span>
    <span class="c1"># ------------------------------------------------------------------------- #       </span>
   
            
<div class="viewcode-block" id="Plucker.intersects"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.intersects">[docs]</a>    <span class="k">def</span> <span class="nf">intersects</span><span class="p">(</span><span class="n">l1</span><span class="p">,</span> <span class="n">l2</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Intersection point of two lines</span>
<span class="sd">        </span>
<span class="sd">        :param l1: First line</span>
<span class="sd">        :type l1: Plucker</span>
<span class="sd">        :param l2: Second line</span>
<span class="sd">        :type l2: Plucker</span>
<span class="sd">        :return: 3D intersection point</span>
<span class="sd">        :rtype: numpy.ndarray, shape=(3,) or None</span>

<span class="sd">        ``l1.intersects(l2)`` is the point of intersection of the two lines, or</span>
<span class="sd">        ``None`` if the lines do not intersect or are equivalent.</span>


<span class="sd">        :seealso: Plucker.commonperp, Plucker.eq, Plucker.__xor__</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="n">l1</span><span class="o">^</span><span class="n">l2</span><span class="p">:</span>
            <span class="c1"># lines do intersect</span>
            <span class="k">return</span> <span class="o">-</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">l1</span><span class="o">.</span><span class="n">v</span><span class="p">,</span> <span class="n">l2</span><span class="o">.</span><span class="n">w</span><span class="p">)</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span> <span class="o">+</span> \
                  <span class="n">l1</span><span class="o">.</span><span class="n">w</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span><span class="mi">1</span><span class="p">))</span> <span class="o">@</span> <span class="n">l2</span><span class="o">.</span><span class="n">v</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="mi">1</span><span class="p">,</span><span class="mi">3</span><span class="p">))</span> <span class="o">-</span> \
                  <span class="n">l2</span><span class="o">.</span><span class="n">w</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span><span class="mi">1</span><span class="p">))</span> <span class="o">@</span> <span class="n">l1</span><span class="o">.</span><span class="n">v</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="mi">1</span><span class="p">,</span><span class="mi">3</span><span class="p">)))</span> <span class="o">*</span> <span class="n">sm</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">l1</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">l2</span><span class="o">.</span><span class="n">w</span><span class="p">))</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1"># lines don&#39;t intersect</span>
            <span class="k">return</span> <span class="kc">None</span></div>
    
<div class="viewcode-block" id="Plucker.distance"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.distance">[docs]</a>    <span class="k">def</span> <span class="nf">distance</span><span class="p">(</span><span class="n">l1</span><span class="p">,</span> <span class="n">l2</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Minimum distance between lines</span>
<span class="sd">        </span>
<span class="sd">        :param l1: First line</span>
<span class="sd">        :type l1: Plucker</span>
<span class="sd">        :param l2: Second line</span>
<span class="sd">        :type l2: Plucker</span>
<span class="sd">        :return: Closest distance</span>
<span class="sd">        :rtype: float</span>

<span class="sd">        ``l1.distance(l2) is the minimum distance between two lines.</span>
<span class="sd">        </span>
<span class="sd">        Notes:</span>
<span class="sd">            </span>
<span class="sd">         - Works for parallel, skew and intersecting lines.</span>
<span class="sd">         &quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="n">l1</span> <span class="o">|</span> <span class="n">l2</span><span class="p">:</span>
            <span class="c1"># lines are parallel</span>
            <span class="n">l</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">l1</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">l1</span><span class="o">.</span><span class="n">v</span> <span class="o">-</span> <span class="n">l2</span><span class="o">.</span><span class="n">v</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">l1</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">l2</span><span class="o">.</span><span class="n">w</span><span class="p">)</span> <span class="o">/</span> <span class="n">dot</span><span class="p">(</span><span class="n">l2</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">l2</span><span class="o">.</span><span class="n">w</span><span class="p">))</span> <span class="o">/</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">l1</span><span class="o">.</span><span class="n">w</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1"># lines are not parallel</span>
            <span class="k">if</span> <span class="nb">abs</span><span class="p">(</span><span class="n">l1</span> <span class="o">*</span> <span class="n">l2</span><span class="p">)</span> <span class="o">&lt;</span> <span class="mi">10</span><span class="o">*</span><span class="n">_eps</span><span class="p">:</span>
                <span class="c1"># lines intersect at a point</span>
                <span class="n">l</span> <span class="o">=</span> <span class="mi">0</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="c1"># lines don&#39;t intersect, find closest distance</span>
                <span class="n">l</span> <span class="o">=</span> <span class="nb">abs</span><span class="p">(</span><span class="n">l1</span> <span class="o">*</span> <span class="n">l2</span><span class="p">)</span> <span class="o">/</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">l1</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">l2</span><span class="o">.</span><span class="n">w</span><span class="p">))</span><span class="o">**</span><span class="mi">2</span>
        <span class="k">return</span> <span class="n">l</span></div>

    
<div class="viewcode-block" id="Plucker.closest"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.closest">[docs]</a>    <span class="k">def</span> <span class="nf">closest</span><span class="p">(</span><span class="n">line</span><span class="p">,</span> <span class="n">x</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Point on line closest to given point</span>
<span class="sd">        </span>
<span class="sd">        :param line: A line</span>
<span class="sd">        :type l1: Plucker</span>
<span class="sd">        :param l2: An arbitrary 3D point</span>
<span class="sd">        :type l2: 3-element array_like</span>
<span class="sd">        :return: Point on the line and distance to line</span>
<span class="sd">        :rtype: collections.namedtuple</span>

<span class="sd">        - ``line.closest(x).p`` is the coordinate of a point on the line that is</span>
<span class="sd">          closest to ``x``.</span>

<span class="sd">        - ``line.closest(x).d`` is the distance between the point on the line and ``x``.</span>
<span class="sd">        </span>
<span class="sd">        The return value is a named tuple with elements:</span>
<span class="sd">            </span>
<span class="sd">            - ``.p`` for the point on the line as a numpy.ndarray, shape=(3,)</span>
<span class="sd">            - ``.d`` for the distance to the point from ``x``</span>
<span class="sd">            - ``.lam`` the `lambda` value for the point on the line.</span>

<span class="sd">        :seealso: Plucker.point</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="c1"># http://www.ahinson.com/algorithms_general/Sections/Geometry/PluckerLine.pdf</span>
        <span class="c1"># has different equation for moment, the negative</span>

        <span class="n">x</span> <span class="o">=</span> <span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span>

        <span class="n">lam</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">x</span> <span class="o">-</span> <span class="n">line</span><span class="o">.</span><span class="n">pp</span><span class="p">,</span> <span class="n">line</span><span class="o">.</span><span class="n">uw</span><span class="p">)</span>
        <span class="n">p</span> <span class="o">=</span> <span class="n">line</span><span class="o">.</span><span class="n">point</span><span class="p">(</span><span class="n">lam</span><span class="p">)</span><span class="o">.</span><span class="n">flatten</span><span class="p">()</span>  <span class="c1"># is the closest point on the line</span>
        <span class="n">d</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span> <span class="n">x</span> <span class="o">-</span> <span class="n">p</span><span class="p">)</span>
        
        <span class="k">return</span> <span class="n">namedtuple</span><span class="p">(</span><span class="s1">&#39;closest&#39;</span><span class="p">,</span> <span class="s1">&#39;p d lam&#39;</span><span class="p">)(</span><span class="n">p</span><span class="p">,</span> <span class="n">d</span><span class="p">,</span> <span class="n">lam</span><span class="p">)</span></div>
    
    
<div class="viewcode-block" id="Plucker.commonperp"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.commonperp">[docs]</a>    <span class="k">def</span> <span class="nf">commonperp</span><span class="p">(</span><span class="n">l1</span><span class="p">,</span> <span class="n">l2</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Common perpendicular to two lines</span>
<span class="sd">        </span>
<span class="sd">        :param l1: First line</span>
<span class="sd">        :type l1: Plucker</span>
<span class="sd">        :param l2: Second line</span>
<span class="sd">        :type l2: Plucker</span>
<span class="sd">        :return: Perpendicular line</span>
<span class="sd">        :rtype: Plucker or None</span>

<span class="sd">        ``l1.commonperp(l2)`` is the common perpendicular line between the two lines.</span>
<span class="sd">        Returns ``None`` if the lines are parallel.</span>

<span class="sd">        :seealso: Plucker.intersect</span>
<span class="sd">        &quot;&quot;&quot;</span>
        
        <span class="k">if</span> <span class="n">l1</span> <span class="o">|</span> <span class="n">l2</span><span class="p">:</span>
            <span class="c1"># no common perpendicular if lines are parallel</span>
            <span class="k">return</span> <span class="kc">None</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1"># lines are skew or intersecting</span>
            <span class="n">w</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">l1</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">l2</span><span class="o">.</span><span class="n">w</span><span class="p">)</span>
            <span class="n">v</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">l1</span><span class="o">.</span><span class="n">v</span><span class="p">,</span> <span class="n">l2</span><span class="o">.</span><span class="n">w</span><span class="p">)</span> <span class="o">-</span> <span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">l2</span><span class="o">.</span><span class="n">v</span><span class="p">,</span> <span class="n">l1</span><span class="o">.</span><span class="n">w</span><span class="p">)</span> <span class="o">+</span> \
                <span class="p">(</span><span class="n">l1</span> <span class="o">*</span> <span class="n">l2</span><span class="p">)</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">l1</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">l2</span><span class="o">.</span><span class="n">w</span><span class="p">)</span> <span class="o">*</span> <span class="n">sm</span><span class="o">.</span><span class="n">unitvec</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">l1</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">l2</span><span class="o">.</span><span class="n">w</span><span class="p">))</span>
            
        <span class="k">return</span> <span class="n">Plucker</span><span class="p">(</span><span class="n">v</span><span class="p">,</span> <span class="n">w</span><span class="p">)</span></div>


<div class="viewcode-block" id="Plucker.__mul__"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.__mul__">[docs]</a>    <span class="k">def</span> <span class="fm">__mul__</span><span class="p">(</span><span class="n">left</span><span class="p">,</span> <span class="n">right</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Reciprocal product</span>
<span class="sd">        </span>
<span class="sd">        :param left: Left operand</span>
<span class="sd">        :type left: Plucker</span>
<span class="sd">        :param right: Right operand</span>
<span class="sd">        :type right: Plucker</span>
<span class="sd">        :return: reciprocal product</span>
<span class="sd">        :rtype: float</span>

<span class="sd">        ``left * right`` is the scalar reciprocal product :math:`\hat{w}_L \dot m_R + \hat{w}_R \dot m_R`.</span>

<span class="sd">        Notes:</span>
<span class="sd">            </span>
<span class="sd">         - Multiplication or composition of Plucker lines is not defined.</span>
<span class="sd">         - Pre-multiplication by an SE3 object is supported, see ``__rmul__``.</span>

<span class="sd">        :seealso: Plucker.__rmul__</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">right</span><span class="p">,</span> <span class="n">Plucker</span><span class="p">):</span>
            <span class="c1"># reciprocal product</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">left</span><span class="o">.</span><span class="n">uw</span><span class="p">,</span> <span class="n">right</span><span class="o">.</span><span class="n">v</span><span class="p">)</span> <span class="o">+</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">right</span><span class="o">.</span><span class="n">uw</span><span class="p">,</span> <span class="n">left</span><span class="o">.</span><span class="n">v</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;bad arguments&#39;</span><span class="p">)</span></div>
        
<div class="viewcode-block" id="Plucker.__rmul__"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.__rmul__">[docs]</a>    <span class="k">def</span> <span class="fm">__rmul__</span><span class="p">(</span><span class="n">right</span><span class="p">,</span> <span class="n">left</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Line transformation</span>

<span class="sd">        :param left: Rigid-body transform</span>
<span class="sd">        :type left: SE3</span>
<span class="sd">        :param right: Right operand</span>
<span class="sd">        :type right: Plucker</span>
<span class="sd">        :return: transformed line</span>
<span class="sd">        :rtype: Plucker</span>
<span class="sd">        </span>
<span class="sd">        ``T * line`` is the line transformed by the rigid body transformation ``T``.</span>


<span class="sd">        :seealso: Plucker.__mul__</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="k">if</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">left</span><span class="p">,</span> <span class="n">SE3</span><span class="p">):</span>
            <span class="n">A</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">r_</span><span class="p">[</span> <span class="n">np</span><span class="o">.</span><span class="n">c_</span><span class="p">[</span><span class="n">left</span><span class="o">.</span><span class="n">R</span><span class="p">,</span>          <span class="n">sm</span><span class="o">.</span><span class="n">skew</span><span class="p">(</span><span class="o">-</span><span class="n">left</span><span class="o">.</span><span class="n">t</span><span class="p">)</span> <span class="o">@</span> <span class="n">left</span><span class="o">.</span><span class="n">R</span><span class="p">],</span>
                       <span class="n">np</span><span class="o">.</span><span class="n">c_</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span><span class="mi">3</span><span class="p">)),</span> <span class="n">left</span><span class="o">.</span><span class="n">R</span><span class="p">]</span>
                        <span class="p">]</span>
            <span class="k">return</span> <span class="n">Plucker</span><span class="p">(</span> <span class="n">A</span> <span class="o">@</span> <span class="n">right</span><span class="o">.</span><span class="n">vec</span><span class="p">)</span>  <span class="c1"># premultiply by SE3</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;bad arguments&#39;</span><span class="p">)</span></div>

    <span class="c1"># ------------------------------------------------------------------------- #</span>
    <span class="c1">#  PLUCKER LINE DISTANCE AND INTERSECTION</span>
    <span class="c1"># ------------------------------------------------------------------------- #       </span>


<div class="viewcode-block" id="Plucker.intersect_plane"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.intersect_plane">[docs]</a>    <span class="k">def</span> <span class="nf">intersect_plane</span><span class="p">(</span><span class="n">line</span><span class="p">,</span> <span class="n">plane</span><span class="p">):</span>
        <span class="sa">r</span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Line intersection with a plane</span>
<span class="sd">        </span>
<span class="sd">        :param line: A line</span>
<span class="sd">        :type line: Plucker</span>
<span class="sd">        :param plane: A plane</span>
<span class="sd">        :type plane: 4-element array_like or Plane</span>
<span class="sd">        :return: Intersection point</span>
<span class="sd">        :rtype: collections.namedtuple</span>

<span class="sd">        - ``line.intersect_plane(plane).p`` is the point where the line </span>
<span class="sd">          intersects the plane, or None if no intersection.</span>
<span class="sd">         </span>
<span class="sd">        - ``line.intersect_plane(plane).lam`` is the `lambda` value for the point on the line</span>
<span class="sd">          that intersects the plane.</span>

<span class="sd">        The plane can be specified as:</span>
<span class="sd">            </span>
<span class="sd">         - a 4-vector :math:`[a, b, c, d]` which describes the plane :math:`\pi: ax + by + cz + d=0`.</span>
<span class="sd">         - a ``Plane`` object</span>
<span class="sd">         </span>
<span class="sd">         The return value is a named tuple with elements:</span>
<span class="sd">            </span>
<span class="sd">            - ``.p`` for the point on the line as a numpy.ndarray, shape=(3,)</span>
<span class="sd">            - ``.lam`` the `lambda` value for the point on the line.</span>

<span class="sd">        See also Plucker.point.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        
        <span class="c1"># Line U, V</span>
        <span class="c1"># Plane N n</span>
        <span class="c1"># (VxN-nU:U.N)</span>
        <span class="c1"># Note that this is in homogeneous coordinates.</span>
        <span class="c1">#    intersection of plane (n,p) with the line (v,p)</span>
        <span class="c1">#    returns point and line parameter</span>
        
        <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">plane</span><span class="p">,</span> <span class="n">Plane</span><span class="p">):</span>
            <span class="n">plane</span> <span class="o">=</span> <span class="n">Plane</span><span class="p">(</span><span class="n">arg</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">plane</span><span class="p">,</span> <span class="mi">4</span><span class="p">))</span>
            
        <span class="n">den</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">line</span><span class="o">.</span><span class="n">w</span><span class="p">,</span> <span class="n">plane</span><span class="o">.</span><span class="n">n</span><span class="p">)</span>
        
        <span class="k">if</span> <span class="nb">abs</span><span class="p">(</span><span class="n">den</span><span class="p">)</span> <span class="o">&gt;</span> <span class="p">(</span><span class="mi">100</span><span class="o">*</span><span class="n">_eps</span><span class="p">):</span>
            <span class="c1"># P = -(np.cross(line.v, plane.n) + plane.d * line.w) / den</span>
            <span class="n">p</span> <span class="o">=</span> <span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">cross</span><span class="p">(</span><span class="n">line</span><span class="o">.</span><span class="n">v</span><span class="p">,</span> <span class="n">plane</span><span class="o">.</span><span class="n">n</span><span class="p">)</span> <span class="o">-</span> <span class="n">plane</span><span class="o">.</span><span class="n">d</span> <span class="o">*</span> <span class="n">line</span><span class="o">.</span><span class="n">w</span><span class="p">)</span> <span class="o">/</span> <span class="n">den</span>
            
            <span class="n">t</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span> <span class="n">line</span><span class="o">.</span><span class="n">pp</span> <span class="o">-</span> <span class="n">p</span><span class="p">,</span> <span class="n">plane</span><span class="o">.</span><span class="n">n</span><span class="p">)</span>
            <span class="k">return</span> <span class="n">namedtuple</span><span class="p">(</span><span class="s1">&#39;intersect_plane&#39;</span><span class="p">,</span> <span class="s1">&#39;p lam&#39;</span><span class="p">)(</span><span class="n">p</span><span class="p">,</span> <span class="n">t</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="kc">None</span></div>

<div class="viewcode-block" id="Plucker.intersect_volume"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.intersect_volume">[docs]</a>    <span class="k">def</span> <span class="nf">intersect_volume</span><span class="p">(</span><span class="n">line</span><span class="p">,</span> <span class="n">bounds</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Line intersection with a volume</span>
<span class="sd">        </span>
<span class="sd">        :param line: A line</span>
<span class="sd">        :type line: Plucker</span>
<span class="sd">        :param bounds: Bounds of an axis-aligned rectangular cuboid</span>
<span class="sd">        :type plane: 6-element array_like</span>
<span class="sd">        :return: Intersection point</span>
<span class="sd">        :rtype: collections.namedtuple</span>
<span class="sd">        </span>
<span class="sd">        ``line.intersect_volume(bounds).p`` is a matrix (3xN) with columns</span>
<span class="sd">        that indicate where the line intersects the faces of the volume</span>
<span class="sd">        specified by ``bounds`` = [xmin xmax ymin ymax zmin zmax].  The number of</span>
<span class="sd">        columns N is either:</span>
<span class="sd">            </span>
<span class="sd">        - 0, when the line is outside the plot volume or,</span>
<span class="sd">        - 2 when the line pierces the bounding volume.</span>
<span class="sd">        </span>
<span class="sd">        ``line.intersect_volume(bounds).lam`` is an array of shape=(N,) where</span>
<span class="sd">        N is as above.</span>
<span class="sd">            </span>
<span class="sd">        The return value is a named tuple with elements:</span>
<span class="sd">            </span>
<span class="sd">            - ``.p`` for the points on the line as a numpy.ndarray, shape=(3,N)</span>
<span class="sd">            - ``.lam`` for the `lambda` values for the intersection points as a</span>
<span class="sd">              numpy.ndarray, shape=(N,).</span>
<span class="sd">        </span>
<span class="sd">        See also Plucker.plot, Plucker.point.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        
        <span class="n">intersections</span> <span class="o">=</span> <span class="p">[]</span>
        
        <span class="c1"># reshape, top row is minimum, bottom row is maximum</span>
        <span class="n">bounds23</span> <span class="o">=</span> <span class="n">bounds</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span> <span class="mi">2</span><span class="p">))</span>
        
        <span class="k">for</span> <span class="n">face</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">6</span><span class="p">):</span>
            <span class="c1"># for each face of the bounding volume</span>
            <span class="c1">#  x=xmin, x=xmax, y=ymin, y=ymax, z=zmin, z=zmax</span>
            
            <span class="n">i</span> <span class="o">=</span> <span class="n">face</span> <span class="o">//</span> <span class="mi">2</span>  <span class="c1"># 0, 1, 2</span>
            <span class="n">I</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">,</span><span class="mi">3</span><span class="p">)</span>
            <span class="n">p</span> <span class="o">=</span> <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span>
            <span class="n">p</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="o">=</span> <span class="n">bounds</span><span class="p">[</span><span class="n">face</span><span class="p">]</span>
            <span class="n">plane</span> <span class="o">=</span> <span class="n">Plane</span><span class="o">.</span><span class="n">PN</span><span class="p">(</span><span class="n">n</span><span class="o">=</span><span class="n">I</span><span class="p">[:,</span><span class="n">i</span><span class="p">],</span> <span class="n">p</span><span class="o">=</span><span class="n">p</span><span class="p">)</span>
            
            <span class="c1"># find where line pierces the plane</span>
            <span class="k">try</span><span class="p">:</span>
                <span class="n">p</span><span class="p">,</span> <span class="n">lam</span> <span class="o">=</span> <span class="n">line</span><span class="o">.</span><span class="n">intersect_plane</span><span class="p">(</span><span class="n">plane</span><span class="p">)</span>
            <span class="k">except</span> <span class="ne">TypeError</span><span class="p">:</span>
                <span class="k">continue</span>  <span class="c1"># no intersection with this plane</span>
            
<span class="c1">#            # print(&#39;face %d: n=(%f, %f, %f), p=(%f, %f, %f)&#39; % (face, plane.n, plane.p))</span>
<span class="c1">#            print(&#39;      : p=(%f, %f, %f)  &#39; % p)</span>
            
            <span class="c1"># print(&#39;face&#39;, face, &#39; point &#39;, p, &#39; plane &#39;, plane)</span>
            <span class="c1"># find if intersection point is within the cube face</span>
            <span class="c1">#  test x,y,z simultaneously</span>
            <span class="n">k</span> <span class="o">=</span> <span class="p">(</span><span class="n">p</span> <span class="o">&gt;=</span> <span class="n">bounds23</span><span class="p">[:,</span><span class="mi">0</span><span class="p">])</span> <span class="o">&amp;</span> <span class="p">(</span><span class="n">p</span> <span class="o">&lt;=</span> <span class="n">bounds23</span><span class="p">[:,</span><span class="mi">1</span><span class="p">])</span>
            <span class="n">k</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">delete</span><span class="p">(</span><span class="n">k</span><span class="p">,</span> <span class="n">i</span><span class="p">)</span>  <span class="c1"># remove the boolean corresponding to current face</span>
            <span class="k">if</span> <span class="nb">all</span><span class="p">(</span><span class="n">k</span><span class="p">):</span>
                <span class="c1"># if within bounds, add</span>
                <span class="n">intersections</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">lam</span><span class="p">)</span>
                
<span class="c1">#                     print(&#39;  HIT&#39;);</span>

        <span class="c1"># put them in ascending order</span>
        <span class="n">intersections</span><span class="o">.</span><span class="n">sort</span><span class="p">()</span>
        
        <span class="n">p</span> <span class="o">=</span> <span class="n">line</span><span class="o">.</span><span class="n">point</span><span class="p">(</span><span class="n">intersections</span><span class="p">)</span>
        
        <span class="k">return</span> <span class="n">namedtuple</span><span class="p">(</span><span class="s1">&#39;intersect_volume&#39;</span><span class="p">,</span> <span class="s1">&#39;p lam&#39;</span><span class="p">)(</span><span class="n">p</span><span class="p">,</span> <span class="n">intersections</span><span class="p">)</span></div>

    
    <span class="c1"># ------------------------------------------------------------------------- #</span>
    <span class="c1">#  PLOT AND DISPLAY</span>
    <span class="c1"># ------------------------------------------------------------------------- #   </span>
    
<div class="viewcode-block" id="Plucker.plot"><a class="viewcode-back" href="../../spatialmath.html#spatialmath.geom3d.Plucker.plot">[docs]</a>    <span class="k">def</span> <span class="nf">plot</span><span class="p">(</span><span class="n">line</span><span class="p">,</span> <span class="n">bounds</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">         Plot a line</span>
<span class="sd">         </span>
<span class="sd">        :param line: A line</span>
<span class="sd">        :type line: Plucker</span>
<span class="sd">        :param bounds: Bounds of an axis-aligned rectangular cuboid as [xmin xmax ymin ymax zmin zmax], optional</span>
<span class="sd">        :type plane: 6-element array_like</span>
<span class="sd">        :param **kwargs: Extra arguents passed to `Line2D &lt;https://matplotlib.org/3.2.2/api/_as_gen/matplotlib.lines.Line2D.html#matplotlib.lines.Line2D&gt;`_</span>
<span class="sd">        :return: Plotted line</span>
<span class="sd">        :rtype: Line3D or None</span>

<span class="sd">        - ``line.plot(bounds)`` adds a line segment to the current axes, and the handle of the line is returned.  </span>
<span class="sd">          The line segment is defined by the intersection of the line and the given rectangular cuboid. </span>
<span class="sd">          If the line does not intersect the plotting volume None is returned.</span>
<span class="sd">          </span>
<span class="sd">        - ``line.plot()`` as above but the bounds are taken from the axis limits of the current axes.</span>
<span class="sd">          </span>
<span class="sd">        The line color or style is specified by:</span>
<span class="sd">        </span>
<span class="sd">            - a  MATLAB-style linestyle like &#39;k--&#39;</span>
<span class="sd">            - additional arguments passed to `Line2D &lt;https://matplotlib.org/3.2.2/api/_as_gen/matplotlib.lines.Line2D.html#matplotlib.lines.Line2D&gt;`_</span>
<span class="sd">            </span>
<span class="sd">        :seealso: Plucker.intersect_volume</span>
<span class="sd">        &quot;&quot;&quot;</span>
        
        <span class="k">if</span> <span class="n">bounds</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
            <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">gca</span><span class="p">()</span>
            <span class="n">bounds</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">r_</span><span class="p">[</span><span class="n">ax</span><span class="o">.</span><span class="n">get_xlim</span><span class="p">(),</span> <span class="n">ax</span><span class="o">.</span><span class="n">get_ylim</span><span class="p">(),</span> <span class="n">ax</span><span class="o">.</span><span class="n">get_zlim</span><span class="p">()]</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">set_xlim</span><span class="p">(</span><span class="n">bounds</span><span class="p">[:</span><span class="mi">2</span><span class="p">])</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">set_ylim</span><span class="p">(</span><span class="n">bounds</span><span class="p">[</span><span class="mi">2</span><span class="p">:</span><span class="mi">4</span><span class="p">])</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">set_zlim</span><span class="p">(</span><span class="n">bounds</span><span class="p">[</span><span class="mi">4</span><span class="p">:</span><span class="mi">6</span><span class="p">])</span>
        
        <span class="c1">#U = self.Q - self.P;</span>
        <span class="c1">#line.p = self.P; line.v = unit(U);</span>
        
        <span class="n">P</span><span class="p">,</span> <span class="n">lam</span> <span class="o">=</span> <span class="n">line</span><span class="o">.</span><span class="n">intersect_volume</span><span class="p">(</span><span class="n">bounds</span><span class="p">)</span>
        
        <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="n">lam</span><span class="p">)</span> <span class="o">&gt;</span> <span class="mi">0</span><span class="p">:</span>
            <span class="k">return</span> <span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">P</span><span class="p">[</span><span class="mi">0</span><span class="p">,:],</span> <span class="n">P</span><span class="p">[</span><span class="mi">1</span><span class="p">,:],</span> <span class="n">P</span><span class="p">[</span><span class="mi">2</span><span class="p">,:],</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="kc">None</span></div>

    <span class="k">def</span> <span class="fm">__str__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Convert to a string</span>
<span class="sd">        </span>
<span class="sd">        :return: String representation of line parameters</span>
<span class="sd">        :rtype: str</span>

<span class="sd">        ``str(line)`` is a string showing Plucker parameters in a compact single</span>
<span class="sd">        line format like::</span>
<span class="sd">            </span>
<span class="sd">            { 0 0 0; -1 -2 -3}</span>
<span class="sd">            </span>
<span class="sd">        where the first three numbers are the moment, and the last three are the </span>
<span class="sd">        direction vector.</span>

<span class="sd">        &quot;&quot;&quot;</span>
        
        <span class="k">return</span> <span class="s1">&#39;</span><span class="se">\n</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">join</span><span class="p">([</span><span class="s1">&#39;{{ </span><span class="si">{:.5g}</span><span class="s1"> </span><span class="si">{:.5g}</span><span class="s1"> </span><span class="si">{:.5g}</span><span class="s1">; </span><span class="si">{:.5g}</span><span class="s1"> </span><span class="si">{:.5g}</span><span class="s1"> </span><span class="si">{:.5g}</span><span class="s1">}}&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="o">*</span><span class="nb">list</span><span class="p">(</span><span class="n">x</span><span class="o">.</span><span class="n">vec</span><span class="p">))</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="bp">self</span><span class="p">])</span>

    <span class="k">def</span> <span class="fm">__repr__</span><span class="p">(</span><span class="bp">self</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        %Twist.display Display parameters</span>
<span class="sd">        %</span>
<span class="sd">L.display() displays the twist parameters in compact single line format.  If L is a</span>
<span class="sd">vector of Twist objects displays one line per element.</span>
<span class="sd">        %</span>
<span class="sd">Notes::</span>
<span class="sd">- This method is invoked implicitly at the command line when the result</span>
<span class="sd">  of an expression is a Twist object and the command has no trailing</span>
<span class="sd">  semicolon.</span>
<span class="sd">        %</span>
<span class="sd">See also Twist.char.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        
        <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="bp">self</span><span class="p">)</span> <span class="o">==</span> <span class="mi">1</span><span class="p">:</span>
            <span class="k">return</span> <span class="s2">&quot;Plucker([</span><span class="si">{:.5g}</span><span class="s2">, </span><span class="si">{:.5g}</span><span class="s2">, </span><span class="si">{:.5g}</span><span class="s2">, </span><span class="si">{:.5g}</span><span class="s2">, </span><span class="si">{:.5g}</span><span class="s2">, </span><span class="si">{:.5g}</span><span class="s2">])&quot;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="o">*</span><span class="nb">list</span><span class="p">(</span><span class="bp">self</span><span class="p">))</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="s2">&quot;Plucker([</span><span class="se">\n</span><span class="s2">&quot;</span> <span class="o">+</span> \
                <span class="s1">&#39;,</span><span class="se">\n</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">join</span><span class="p">([</span><span class="s2">&quot;  [</span><span class="si">{:.5g}</span><span class="s2">, </span><span class="si">{:.5g}</span><span class="s2">, </span><span class="si">{:.5g}</span><span class="s2">, </span><span class="si">{:.5g}</span><span class="s2">, </span><span class="si">{:.5g}</span><span class="s2">, </span><span class="si">{:.5g}</span><span class="s2">]&quot;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="o">*</span><span class="nb">list</span><span class="p">(</span><span class="n">tw</span><span class="p">))</span> <span class="k">for</span> <span class="n">tw</span> <span class="ow">in</span> <span class="bp">self</span><span class="p">])</span> <span class="o">+</span>\
                <span class="s2">&quot;</span><span class="se">\n</span><span class="s2">])&quot;</span></div>
        
        
<span class="c1">#         function z = side(self1, pl2)</span>
<span class="c1">#             Plucker.side Plucker side operator</span>
<span class="c1"># </span>
<span class="c1">#             # X = SIDE(P1, P2) is the side operator which is zero whenever</span>
<span class="c1">#             # the lines P1 and P2 intersect or are parallel.</span>
<span class="c1"># </span>
<span class="c1">#             # See also Plucker.or.</span>
<span class="c1">#             </span>
<span class="c1">#             if ~isa(self2, &#39;Plucker&#39;)</span>
<span class="c1">#                 error(&#39;SMTB:Plucker:badarg&#39;, &#39;both arguments to | must be Plucker objects&#39;);</span>
<span class="c1">#             end</span>
<span class="c1">#             L1 = pl1.line(); L2 = pl2.line();</span>
<span class="c1">#             </span>
<span class="c1">#             z = L1([1 5 2 6 3 4]) * L2([5 1 6 2 4 3])&#39;;</span>
<span class="c1">#         end</span>

<span class="c1">#         </span>
<span class="c1">#         function z = intersect(self1, pl2)</span>
<span class="c1">#             Plucker.intersect  Line intersection</span>
<span class="c1">#             </span>
<span class="c1">#             PL1.intersect(self2) is zero if the lines intersect.  It is positive if PL2</span>
<span class="c1">#             passes counterclockwise and negative if PL2 passes clockwise.  Defined as</span>
<span class="c1">#             looking in direction of PL1</span>
<span class="c1">#             </span>
<span class="c1">#                                        ----------&gt;</span>
<span class="c1">#                            o                o</span>
<span class="c1">#                       ----------&gt;</span>
<span class="c1">#                      counterclockwise    clockwise</span>
<span class="c1">#             </span>
<span class="c1">#             z = dot(self1.w, pl1.v) + dot(self2.w, pl2.v);</span>
<span class="c1">#         end</span>
        
    <span class="c1"># Static factory methods for constructors from exotic representations</span>


    
<span class="k">if</span> <span class="vm">__name__</span> <span class="o">==</span> <span class="s1">&#39;__main__&#39;</span><span class="p">:</span>   <span class="c1"># pragma: no cover</span>

    <span class="kn">import</span> <span class="nn">pathlib</span>
    <span class="kn">import</span> <span class="nn">os.path</span>
    
    <span class="n">a</span> <span class="o">=</span> <span class="n">SE3</span><span class="o">.</span><span class="n">Exp</span><span class="p">([</span><span class="mi">2</span><span class="p">,</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">])</span>

    <span class="n">exec</span><span class="p">(</span><span class="nb">open</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">pathlib</span><span class="o">.</span><span class="n">Path</span><span class="p">(</span><span class="vm">__file__</span><span class="p">)</span><span class="o">.</span><span class="n">parent</span><span class="o">.</span><span class="n">absolute</span><span class="p">(),</span> <span class="s2">&quot;test_geom3d.py&quot;</span><span class="p">))</span><span class="o">.</span><span class="n">read</span><span class="p">())</span>
</pre></div>

          </div>
          
        </div>
      </div>
      <div class="sphinxsidebar" role="navigation" aria-label="main navigation">
        <div class="sphinxsidebarwrapper">
<p class="logo">
  <a href="../../index.html">
    <img class="logo" src="../../_static/../../../../figs/icon.png" alt="Logo"/>
    
  </a>
</p>



<p class="blurb">Spatial maths and geometry for Python</p>




<p>
<iframe src="https://ghbtns.com/github-btn.html?user=petercorke&repo=spatialmath-python&type=watch&count=true&size=large&v=2"
  allowtransparency="true" frameborder="0" scrolling="0" width="200px" height="35px"></iframe>
</p>





<h3>Navigation</h3>
<ul>
<li class="toctree-l1"><a class="reference internal" href="../../intro.html">Introduction</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../spatialmath.html">Classes and functions</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../indices.html">Indices</a></li>
</ul>

<div class="relations">
<h3>Related Topics</h3>
<ul>
  <li><a href="../../index.html">Documentation overview</a><ul>
  <li><a href="../index.html">Module code</a><ul>
  </ul></li>
  </ul></li>
</ul>
</div>
<div id="searchbox" style="display: none" role="search">
  <h3 id="searchlabel">Quick search</h3>
    <div class="searchformwrapper">
    <form class="search" action="../../search.html" method="get">
      <input type="text" name="q" aria-labelledby="searchlabel" />
      <input type="submit" value="Go" />
    </form>
    </div>
</div>
<script>$('#searchbox').show(0);</script>








        </div>
      </div>
      <div class="clearer"></div>
    </div>
    <div class="footer">
      &copy;2020, Peter Corke.
      
      |
      Powered by <a href="http://sphinx-doc.org/">Sphinx 2.4.4</a>
      &amp; <a href="https://github.com/bitprophet/alabaster">Alabaster 0.7.12</a>
      
    </div>

    

    
  </body>
</html>